#include <stdio.h>
#include <math.h>
#include <float.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_rng.h>

gsl_rng * rsr;  /* global random number generator */


/*
 *  Bounded minimization in (a,b)
 *  xmin		minima
 *  fxmin 		value of objective function at the minima
 *  my_obj_f	objective function
 *  a			left edge of interval
 *  b			right edge of interval
 *  params		parameters passed to the objective function
 *  xtol		tolerance for the minima
 *  maxiter		maximum number of iterations allowed
 */
double bound_min(double *xmin, double *fxmin, double (*my_obj_f)(double , void *), double a, double b, void * params, double xtol, int maxiter) {
	
	double answer = -DBL_MAX;
	int status, iter = 0;
	const gsl_min_fminimizer_type *T;
	gsl_min_fminimizer *s;
	double m;
	double fa = (*my_obj_f) (a, params);
	double fb = (*my_obj_f) (b, params);
	// printf ("Lower a = %.7f, f(a) = %.7f \n", a, fa);
	// printf ("Upper b = %.7f, f(b) = %.7f \n", b, fb);
	if (fa < fb)
		m = a + xtol;
	else
		m = b - xtol;
	// printf ("Min   m = %.7f, f(m) = %.7f \n", m, (*my_obj_f) (m, params));
	
	gsl_function F;
	F.function = my_obj_f;
	F.params = params;
	
	T = gsl_min_fminimizer_brent;
	s = gsl_min_fminimizer_alloc (T);
	gsl_min_fminimizer_set (s, &F, m, a, b);
	
	// printf ("using %s method\n", gsl_min_fminimizer_name (s));
	
	// printf ("%5s [%9s, %9s] %9s %10s %9s\n", "iter", "lower", "upper", "min", "err", "err(est)");
	// printf ("%5d [%.7f, %.7f] %.7f %.7f\n", iter, a, b, m, b - a);
	
	do {
		iter++;
		status = gsl_min_fminimizer_iterate (s);
		
		m = gsl_min_fminimizer_x_minimum (s);
		a = gsl_min_fminimizer_x_lower (s);
		b = gsl_min_fminimizer_x_upper (s);
		
		status = gsl_min_test_interval (a, b, xtol, 0.0); // relative error = 0.0, absolute error = xtol
		
		// if (status == GSL_SUCCESS)
		// printf ("Converged:\n");
		
		// printf ("%5d [%.7f, %.7f] " "%.7f %.7f\n", iter, a, b, m, b - a);
	} while (status == GSL_CONTINUE && iter < maxiter);
	
	if (iter == maxiter)
		printf("Maximum number of iterations reached.\n");
	
	answer = gsl_min_fminimizer_x_minimum (s);
	*xmin = gsl_min_fminimizer_x_minimum (s);
	*fxmin = gsl_min_fminimizer_f_minimum(s) ;
	
	gsl_min_fminimizer_free (s);
	
	return answer;
}


/*
 *  Unbounded minimization 
 *  xmin 		minima
 *  fxmin 		value of the objective function at the minima
 *  my_obj_f	objective function
 *	x_init		initial value 
 *  params		parameters for the objective function
 *  ss			step size
 *  xtol		tolerance 
 *  maxiter		maximum iterations
 */
double unbound_min(double *xmin, double *fxmin, double (*my_obj_f)(const gsl_vector *, void *), double x_init, void* params, double ss,  double xtol, int maxiter) {
	
	const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2;
	gsl_multimin_fminimizer *s = NULL;
	gsl_vector *step, *x, *oldx;
	gsl_multimin_function minex_func;
	
	int iter = 0;	// current iteration
	int status;		// gsl status
	double size;	// minimizer size
	double answer = -DBL_MAX;	// answer
	
    // Set starting point 
	x = gsl_vector_alloc (1);
	oldx = gsl_vector_alloc (1);
	gsl_vector_set (x, 0, x_init);
	gsl_vector_memcpy (oldx, x);
	
    // Set step size
	step = gsl_vector_alloc (1);
	gsl_vector_set_all (step, ss);
	
    // Initialize method and iterate 
	minex_func.n = 1;
	minex_func.f = my_obj_f;
	minex_func.params = params;
	s = gsl_multimin_fminimizer_alloc (T, 1);
	gsl_multimin_fminimizer_set (s, &minex_func, x, step);
	
	do {
		iter++;
		status = gsl_multimin_fminimizer_iterate (s);
		//  printf ("Iterate status = %d\n", status);
		if (status)
			break;
		
		size = gsl_multimin_fminimizer_size (s);
		status = gsl_multimin_test_size (size, xtol);
		// printf ("Test status = %d\n", status);
		
		// if (isinf(gsl_multimin_fminimizer_minimum(s)))
		// printf("Inf value returned by underlying objective function.\n");
		
		// if (status == GSL_SUCCESS) 
		// printf ("converged to minimum at\n");
		
		// printf("%5d \t x = %.7f \t f() = %.7f \t err = %.7f \t min = %.7f \n", iter, gsl_vector_get (s->x, 0), s->fval, size, gsl_multimin_fminimizer_minimum (s));
		
		if (isnan(gsl_multimin_fminimizer_minimum(s))) 
			gsl_multimin_fminimizer_set (s, &minex_func, oldx, step);
		else
			gsl_vector_memcpy (oldx, s->x);
	} while (status == GSL_CONTINUE && iter < maxiter);
	
	if (iter == maxiter)
		printf("Maximum number of iterations reached.\n");
	
	answer = gsl_vector_get(gsl_multimin_fminimizer_x(s), 0);
	*xmin = gsl_vector_get(gsl_multimin_fminimizer_x(s), 0);
	*fxmin = gsl_multimin_fminimizer_minimum(s);
	gsl_vector_free (x);
	gsl_vector_free (step);
	gsl_multimin_fminimizer_free (s);
	
	return answer;
}


/*  function to perform rejection sampling on objective pdf -1*fx
 *  maxima of the enevelope function is found in [xl xu]
 *  if xl==xu, then maxima is found near xl
 *  params is parameters for fx, if required
 *  wf is the width factor that determines the width of the envelope 
 *  mf is the multiplicative factor we use in binary search to grow the interval
 *  tx is the tolerance for the optimization of x
 *  maxitr is the maximum number of iterations of binary search 
 *  another constraint is the fx(x) is defined only for positive x values
 *  WE ASSUME THAT FX IS LOG SPACE FUNCTION
 */
double doRejectionSampling(double (*f_bnd)(double , void *), void* params, double xl, double xu, double wf, double mf, double tx, int maxitr){
	
	// initialize
	double rs = 0, xmax = 0, fxmax = 0;
	double DEL1 = log(0.9*wf);
	double DEL2 = log(1.1*wf);
	double RHO = mf;
	double oxl = xl; double oxu = xu; // save old values
	
	// disp interval
	// printf("\ndoRejectionSampling: Interval (%f %f)\n",xl,xu);
	
	// find the height of the envelope
	bound_min(&xmax, &fxmax, f_bnd, xl, xu, params, tx, maxitr); // height of the envelope
	if (xmax < xl || xmax > xu){
		printf("\ndoRejectionSampling: fminbnd returned envelope height that is out of bounds.\n");
		printf("[%f %f %f]\n",xl, xmax, xu);
	}
	// printf("Height of envelope: xmax=%f \tfxmax=%f\n", xmax, fxmax);
	
	// find the left edge
	double rsminl = 0.5*xmax; // init left bound for left edge
	while ((rsminl >= xl) && ((*f_bnd)(rsminl,params) - fxmax < DEL1)) // keep expanding the left bound
		rsminl = xmax - RHO*(xmax - rsminl);
	if (rsminl < xl) //make sure bounds are met
		rsminl = xl;
	double rsminr = xmax; // init right bound for left edge
	double rsmin = (rsminl + rsminr) / 2; // init left edge as avg of bounds
	int bsitr =0;
	for (bsitr=1; bsitr<=maxitr; bsitr++){ // iterations of binary search for left edge
		double fxdiff = (*f_bnd)(rsmin,params) - fxmax;
		if (fxdiff > DEL2 || isinf(fxdiff) == -1) // we are far too left
			rsminl = rsmin; // update left bound
		else if (fxdiff < DEL1) // we are far too right
			rsminr = rsmin; // update right bound    
		else
			break;
		rsmin = (rsminl + rsminr) / 2; // update left edge value
	}
	// printf("Left side: [%f %f %f %f %f %d]\n", rsmin, (*f_bnd)(rsmin,params), fxmax, (*f_bnd)(rsmin,params) - fxmax, log(wf), bsitr);
	
	// find the right edge
	double rsmaxr = 2*xmax ; // init right bound for the right edge
	while ((rsmaxr <= xu) && ((*f_bnd)(rsmaxr,params) - fxmax < DEL1))
		rsmaxr = xmax + RHO*(rsmaxr - xmax);
	if (rsmaxr > xu) // make sure bounds are met
		rsmaxr = xu;
	double rsmaxl = xmax; // init left bound for the right edge
	double rsmax = (rsmaxl + rsmaxr) / 2; // init right edge as avg of bounds
	for (bsitr=1; bsitr<=maxitr; bsitr++){ // iterations of binary search for right edge
		double fxdiff = (*f_bnd)(rsmax,params) - fxmax;
		if (fxdiff > DEL2 || isinf(fxdiff) == -1) // we are too far to the right
			rsmaxr = rsmax;
		else if (fxdiff < DEL1) // we are too far to the left
			rsmaxl = rsmax;                
		else
			break;
		rsmax = (rsmaxl + rsmaxr) / 2; // update right edge value
	}
	// printf("Right side: [%f %f %f %f %f %d]\n", rsmax, (*f_bnd)(rsmax,params), fxmax, (*f_bnd)(rsmax,params) - fxmax, log(wf), bsitr);
	
	// check all bounds
	double rsdiff = rsmax - rsmin; //width of the envelope
	// printf("[%f %f %f %f %f]\n", xl, rsmin, xmax, rsmax, xu);
	if (rsmin > xmax || xmax > rsmax){
		printf("doRejectionSampling: Could not compute the correct interval for rejection sampling\n");
		return -1;
	}
	if (rsmin < xl || rsmax > xu){
		printf("doRejectionSampling: Could not rejection sampling interval within provided bounds\n");
		return -1;
	}
	
	// perform rejection sampling
	int rscount=0; double xtemp=0;
	// while (true){
	while (rscount < maxitr){
		rscount = rscount + 1;
		xtemp = rsmin + gsl_rng_uniform(rsr) * rsdiff ; // x1 is in range [rsmin rsmax]
		double u = gsl_rng_uniform(rsr); // pick a random number
		if (log(u) <= (-(*f_bnd)(xtemp,params) + fxmax)) // rejection sampling
			break;
	}
	// printf("%d samples needed for rejection sampling. Sample = %f\n",rscount,xtemp);
	if (rscount == maxitr){
		rs = xmax;
		printf("%d samples needed for rejection sampling. \n",rscount);
		printf("[%f %f %f %f %f] %f \n", xl, rsmin, xmax, rsmax, xu, fxmax);
		// getchar();
	} else
		rs=xtemp;
	
	return rs;
}



/*  function to perform rejection sampling on objective pdf -1*fx
 *  maxima of the enevelope function is found near xin
 *  params is parameters for fx, if required
 *  wf is the width factor that determines the width of the envelope 
 *  mf is the multiplicative factor we use in binary search to grow the interval
 *  tx is the tolerance for the optimization of x
 *  maxitr is the maximum number of iterations of binary search 
 *  another constraint is the fx(x) is defined only for positive x values
 *  WE ASSUME THAT FX IS LOG SPACE FUNCTION
 */
double doRejectionSampling(double (*f_unbnd)(const gsl_vector * , void *), void* params, double xin, double wf, double mf, double tx, int maxitr){
	
	// initialize
	double rs = 0, xmax = 0, fxmax = 0;
	gsl_vector *rsx = gsl_vector_calloc(1);
	double DEL1 = log(0.9*wf);
	double DEL2 = log(1.1*wf);
	double RHO = mf;
	double xl = tx, xu = DBL_MAX;
	// double xl = 0, xu = DBL_MAX;
	// double xl = 0, xu = 100;
	
	// disp interval
	// printf("\ndoRejectionSampling: Near  %f \n",xin);
	
	if (xin < tx){
		printf("\ndoRejectionSampling: xin < tx \t %f < %f.\n", xin, tx);
	}
	
	// find the height of the envelope
	unbound_min(&xmax, &fxmax, f_unbnd, xin, params, tx, tx, maxitr); // height of the envelope
	if (xmax < xl || xmax > xu){
		// printf("\ndoRejectionSampling: unbound_min returned envelope height that is out of bounds.\n");
		// printf("[%f %f %f] %f \n",xl, xmax, xu, xin);
		// printf("Setting xmax to xl or xu appropriately, and recomputing fxmax.\n");
		if (xmax < xl)
			xmax = xl;
		else
			xmax = xu;
		gsl_vector_set(rsx, 0, xmax);
		fxmax = (*f_unbnd)(rsx,params);
		// printf("Return value is xin = %f\n",xin);
		// return xin;
		// getchar();
	}
	// printf("Height of envelope: xmax=%f \tfxmax=%f\n", xmax, fxmax);
	
	
	// find the left edge
	double rsminl = 0.5*xmax; gsl_vector_set(rsx, 0, rsminl); // init left bound for left edge
	while ((rsminl >= xl) && ((*f_unbnd)(rsx,params) - fxmax < DEL1)){ // keep expanding the left bound
		rsminl = xmax - RHO*(xmax - rsminl);
		gsl_vector_set(rsx, 0, rsminl);
	}
	if (rsminl < xl) //make sure bounds are met
		rsminl = xl;
	double rsminr = xmax; // init right bound for left edge
	double rsmin = (rsminl + rsminr) / 2; gsl_vector_set(rsx, 0, rsmin); // init left edge as avg of bounds
	int bsitr =0;
	for (bsitr=1; bsitr<=maxitr; bsitr++){ // iterations of binary search for left edge
		double fxdiff = (*f_unbnd)(rsx,params) - fxmax;
		if (fxdiff > DEL2 || isinf(fxdiff) == -1) // we are far too left
			rsminl = rsmin; // update left bound
		else if (fxdiff < DEL1) // we are far too right
			rsminr = rsmin; // update right bound    
		else
			break;
		rsmin = (rsminl + rsminr) / 2; gsl_vector_set(rsx, 0, rsmin); // update left edge value
	}
	// printf("Left side: [%f %f %f %f %f %d]\n", rsmin, (*f_unbnd)(rsx,params), fxmax, (*f_unbnd)(rsx,params) - fxmax, log(wf), bsitr);
	
	// find the right edge
	double rsmaxr = 2*xmax ; gsl_vector_set(rsx, 0, rsmaxr); // init right bound for the right edge
	while ((rsmaxr <= xu) && ((*f_unbnd)(rsx,params) - fxmax < DEL1)){
		rsmaxr = xmax + RHO*(rsmaxr - xmax);
		gsl_vector_set(rsx, 0, rsmaxr);
	}
	if (rsmaxr > xu) // make sure bounds are met
		rsmaxr = xu;
	double rsmaxl = xmax; // init left bound for the right edge
	double rsmax = (rsmaxl + rsmaxr) / 2; gsl_vector_set(rsx, 0, rsmax); // init right edge as avg of bounds
	for (bsitr=1; bsitr<=maxitr; bsitr++){ // iterations of binary search for right edge
		double fxdiff = (*f_unbnd)(rsx,params) - fxmax;
		// printf("\t\t rsmax = %f, f(rsmax) = %f, fxdiff = %f \n", rsmax, (*f_unbnd)(rsx,params), fxdiff); 
		if (fxdiff > DEL2 || isinf(fxdiff) == -1) // we are too far to the right
			rsmaxr = rsmax;
		else if (fxdiff < DEL1) // we are too far to the left
			rsmaxl = rsmax;                
		else
			break;
		rsmax = (rsmaxl + rsmaxr) / 2; gsl_vector_set(rsx, 0, rsmax); // update right edge value
	}
	// printf("Right side: [%f %f %f %f %f %d]\n", rsmax, (*f_unbnd)(rsx,params), fxmax, (*f_unbnd)(rsx,params) - fxmax, log(wf), bsitr);
	
	// check all bounds
	double rsdiff = rsmax - rsmin; //width of the envelope
	// printf("[%f %f %f %f %f]\n", xl, rsmin, xmax, rsmax, xu);
	if (rsmin > xmax || xmax > rsmax){
		printf("doRejectionSampling: Could not compute the correct interval for rejection sampling\n");
		printf("[%f %f %f %f %f]\n", xl, rsmin, xmax, rsmax, xu);
		// return -1;
		return xmax;
	}
	if (rsmin < xl || rsmax > xu){
		printf("doRejectionSampling: Could not perform rejection sampling interval within provided bounds\n");
		printf("[%f %f %f %f %f]\n", xl, rsmin, xmax, rsmax, xu);
		// return -1;
		return xmax;
	}
	
	// perform rejection sampling
	int rscount=0; double xtemp=0;
	//while (true){
	while (rscount < maxitr){
		rscount = rscount + 1;
		xtemp = rsmin + gsl_rng_uniform(rsr) * rsdiff ; // x1 is in range [rsmin rsmax]
		gsl_vector_set(rsx, 0, xtemp);
		double u = gsl_rng_uniform(rsr); // pick a random number
		if (log(u) <= (-(*f_unbnd)(rsx,params) + fxmax)) // rejection sampling
			break;
	}
	// printf("%d samples needed for rejection sampling. Sample = %f\n",rscount,xtemp);
	if (rscount == maxitr){
		rs = xmax;
		printf("%d samples needed for rejection sampling. \n",rscount);
		printf("[%f %f %f %f %f] %f \n", xl, rsmin, xmax, rsmax, xu, fxmax);
		// getchar();
	} else
		rs=xtemp;
	gsl_vector_free(rsx);
	return rs;
}
